/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "canbuscom.h"
#include "qdebug.h"
#include "messagelog.h"
#include "robotcontrol.h"
//#include "robotbase.h"
#include "timetool.h"
#include "canbusdevicedriver.h"
#include "canbusmaganetic.h"
#include "canbusrfid.h"
#include "canbusservodriver.h"
#include "caniodriver.h"
#include "canbusbattery.h"
#include "vrepobject.h"
#include "vrepcontrol.h"
#include <QThread>
#include "ecatbusmanager.h"
//D_TRY_MAX_COUNT=5时，拷贝文件或cpu高时，容易报错．
#define D_TRY_MAX_COUNT 150
#define D_TRY_MAX_COUNT_RFID 150
#define D_TRY_MAX_COUNT_IOMODULE 150
#define D_TRY_MAX_COUNT_BATTERY 500

extern RobotControl* robCtl;


CanbusCom::CanbusCom(QString canConfigFile, QList<QString> xmlFilelist, QList<int> deviceTypeList,
                     QList<int> aliasList, QList<int> canbusIdList,QList<int> deviceIdList,
                     int pthread_priority_in, int pthread_schedule_in)
            :EcatMasterBase(pthread_priority_in,pthread_schedule_in)
{

    canbusDriver= new CanbusDeviceDriver(canConfigFile);
    currentDeviceId=0;
    currentCanbusId=0;


    if(1==initial(xmlFilelist,deviceTypeList,aliasList,canbusIdList,deviceIdList,canbusDriver)
            && true==canbusDriver->isInitialOkStatus())
    {
        initialEcatIoDidoCount();
//        isInitialedOk=true;
    }
    else
    {
        qDebug()<<"CanbusCom  initial error";
        setMasterState(eMasterErr);
    }

    if(false==canbusDriver->isInitialOkStatus())
    {
        qDebug()<<"CanbusCom  initial error";
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1901, -1 ,0);
        setMasterState(eMasterErr);
    }

    QString rootFilePath=D_TCRCONTROLLER_DATA_PATH;
    QString filePath;
    filePath=rootFilePath+GLOBAL_CANOPEN_PATH+"checkConfig.ini";
    int tmpReturn=readCheckConfig(filePath,wkcLostLimit,ecatPeriodTimerOffsetMax,
                    ecatReceiveMax,ecatSendMax,ecatCustomMax);
    if(1!=tmpReturn)
    {
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom", 9048, -1 );
        setMasterState( eMasterErr );
    }


}

CanbusCom::~CanbusCom()
{

}

int CanbusCom::getCanDebugInfo(EthercatDebugInfo &debugInfo)
{
//    debugInfo.slaves_al.resize(isSlaveInOp.size());
//    for(int i=0;i<isSlaveInOp.size();i++)
//    {
//        debugInfo.slaves_al[i]=slavesAl[i];
//    }
    debugInfo.masterCommandState=masterCommandState;
    debugInfo.masterCurrentState=masterCurrentState;
    debugInfo.masteroldState=masteroldState;
    debugInfo.stage=E_ETHERCAT_STAGE_INIT;
    debugInfo.wkc_count=0;
    debugInfo.wkc_lost_max=0;
    debugInfo.wkc_state=0;
    debugInfo.dcTimerShift=system_time_base;//new
    debugInfo.cycle_us=cycle_us;
    debugInfo.opLoopCount=opLoopCount;
    debugInfo.refTime=0;
    debugInfo.rerfSumTime=0;
    debugInfo.pcSumTime=0;
    debugInfo.pcCycleCount=cycle_counter;//new

    debugInfo.ecatPeriodTimerMonitor_now=ecatPeriodTimerMonitor_now;
    debugInfo.ecatPeriodTimerMonitor_max=ecatPeriodTimerMonitor_max;
    debugInfo.ecatPeriodTimerMonitor_min=ecatPeriodTimerMonitor_min;
    debugInfo.ecatReceiveMonitor_now=ecatReceiveMonitor_now;
    debugInfo.ecatReceiveMonitor_max=ecatReceiveMonitor_max;
    debugInfo.ecatReceiveMonitor_min=ecatReceiveMonitor_min;
    debugInfo.ecatSendMonitor_now=ecatSendMonitor_now;
    debugInfo.ecatSendMonitor_max=ecatSendMonitor_max;
    debugInfo.ecatSendMonitor_min=ecatSendMonitor_min;
    debugInfo.ecatCustomMonitor_now=ecatCustomMonitor_now;
    debugInfo.ecatCustomMonitor_max=ecatCustomMonitor_max;
    debugInfo.ecatCustomMonitor_min=ecatCustomMonitor_min;

    return 1;
}

int CanbusCom::writeTagData(int aliasIn, long long cardValueIn)
{
    if(true==rfidDriverMap.contains(aliasIn))
    {
        QVector<unsigned char> tmpCardValue;
        unsigned char tmpUC;
        tmpUC=cardValueIn&0xff;
        tmpCardValue.append(tmpUC);
        tmpUC=(cardValueIn>>8)&0xff;
        tmpCardValue.append(tmpUC);
        tmpUC=(cardValueIn>>16)&0xff;
        tmpCardValue.append(tmpUC);
        tmpUC=(cardValueIn>>24)&0xff;
        tmpCardValue.append(tmpUC);
        return rfidDriverMap.value(aliasIn)->writeTagDataUndirect(tmpCardValue);
    }
    else
    {
        return -1;
    }
}

int CanbusCom::writeTagData(int aliasIn, QVector<unsigned char> cardValueIn)
{
    if(true==rfidDriverMap.contains(aliasIn))
    {
        return rfidDriverMap.value(aliasIn)->writeTagDataUndirect(cardValueIn);
    }
    else
    {
        return -1;
    }
}

int CanbusCom::getCurrentTagData(int aliasIn, long long &cardValueOut, bool &isCardDetected)
{
    QVector<unsigned char> tmpCardValueOut;
    int returnKey=getCurrentTagData(aliasIn,tmpCardValueOut,isCardDetected);
    if(1==returnKey)
    {
        if(false==isCardDetected)
        {
            cardValueOut=(tmpCardValueOut[3]<<24)+(tmpCardValueOut[2]<<16)+
                                                      (tmpCardValueOut[1]<<8)+tmpCardValueOut[0];
    //        cardValueOut=-1;
        }
        else
        {
            cardValueOut=(tmpCardValueOut[3]<<24)+(tmpCardValueOut[2]<<16)+
                                                      (tmpCardValueOut[1]<<8)+tmpCardValueOut[0];
        }
    }
    else
    {
        cardValueOut=-10000;
        isCardDetected=false;
        return -1;
    }

    return 1;
}

int CanbusCom::getCurrentTagData(int aliasIn,QVector<unsigned char> &cardValueOut, bool &isCardDetected)
{
    if(true==rfidDriverMap.contains(aliasIn))
    {
        return rfidDriverMap.value(aliasIn)->getCurrentTagDataFromBuffer(cardValueOut,isCardDetected);
    }
    else
    {
        isCardDetected=false;
        return -1;
    }
}

int CanbusCom::getMaganeticSensorRawValue(int aliasIn, QVector<bool> &sensorRawValueOut)
{
    if(true==maganeticDriverMap.contains(aliasIn))
    {
        return maganeticDriverMap.value(aliasIn)->getSensorRawValueFromBuffer(sensorRawValueOut);
    }
    else
    {
        return -1;
    }


}

int CanbusCom::getBatteryInfo_fromMotor(int aliasIn, BatteryInformation &batteryInfoOut)
{
    if(true==motorDriverMap.contains(aliasIn))
    {
        return motorDriverMap.value(aliasIn)->getBatteryInfo(batteryInfoOut);
    }
    else
    {
        return -1;
    }
}

int CanbusCom::getBatteryInfo(int aliasIn, BatteryInformation &batteryInfoOut)
{
    if(true==batteryDriverMap.contains(aliasIn))
    {
        return batteryDriverMap.value(aliasIn)->getBatteryInfo(batteryInfoOut);
    }
    else
    {
        return -1;
    }
}

int CanbusCom::sdoUpload(int node, int rObIndex, int rObSubIndex, int64_t &rData, int &rDataLen)
{
    int keyReturn = canbusDriver->sdoUpload(currentDeviceId, currentCanbusId,node,
                                            rObIndex,rObSubIndex,rData,rDataLen);
    if(-1==keyReturn)
    {
        qDebug()<<"error, canbus sdoUpload error!";
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1921, -1 );

    }
    if(-2==keyReturn)
    {
        qDebug()<<"error, canbus sdoUpload error!";
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1922, -1 );

    }
    if(-3==keyReturn)
    {
        qDebug()<<"error, canbus sdoUpload error!";
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1923, -1 );

    }
    if(-4==keyReturn)
    {
        qDebug()<<"error, canbus sdoUpload error!";
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1924, -1 );

    }
    if(-5==keyReturn)
    {
        qDebug()<<"error, canbus sdoUpload error!";
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1924, -1 );

    }
    if(-6==keyReturn)
    {
        qDebug()<<"error, canbus sdoUpload error!";
        addMsg(ENUM_MSG_WARNING, "EcatMaster", "CanbusCom" , 1925, -1 );
    }


    return keyReturn;



}

int CanbusCom::sdoDownload(int node, int wObIndex, int wObSubIndex, int64_t wData, int wDataLen)
{
    int keyReturn = canbusDriver->sdoDownload(currentDeviceId, currentCanbusId,node,
                                            wObIndex,wObSubIndex,wData,wDataLen);

    if(-2==keyReturn)
    {
        qDebug()<<"error, canbus sdoUpload error!";
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1921, -1 );

    }
    if(-3==keyReturn)
    {
        qDebug()<<"error, canbus sdoUpload error!";
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1922, -1 );

    }
    if(-4==keyReturn)
    {
        qDebug()<<"error, canbus sdoUpload error!";
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1923, -1 );

    }
    if(-5==keyReturn)
    {
        qDebug()<<"error, canbus sdoUpload error!";
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1924, -1 );

    }
    if(-6==keyReturn)
    {
        qDebug()<<"error, canbus sdoUpload error!";
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1924, -1 );
    }
    if(-7==keyReturn)
    {
        qDebug()<<"error, canbus sdoUpload error!";
        addMsg(ENUM_MSG_WARNING, "EcatMaster", "CanbusCom" , 1925, -1 );
    }

    return keyReturn;

}

int CanbusCom::getMotorVendorId(int node)
{
   return motorDriverMap.value(node)->getVendorId();
}

//int CanbusCom::initialEcatIoDidoCount()
//{

//}



int CanbusCom::initial(QList<QString> xmlFilelist, QList<int> deviceTypeList, QList<int> aliasList
                       , QList<int> canbusIdList,QList<int> deviceIdList ,CanbusDeviceDriver *canDriverIn)
{
    for(int i=0;i<deviceTypeList.size();i++)
    {
        if(E_EcatDev_Servo==deviceTypeList[i])
        {
            CanbusServoDriver *tmpMotorDriver=
                        new CanbusServoDriver(xmlFilelist[i],aliasList[i], canbusIdList[i],deviceIdList[i],canDriverIn);

            if(true!=tmpMotorDriver->isInitialOkStatus())
            {
                addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1942, -1,aliasList[i] );
                return -1;
            }
            motorDriver.append(tmpMotorDriver);
            motorDriverMap.insert(aliasList[i],tmpMotorDriver);
        }
        else if(E_EcatDev_rfidSensor==deviceTypeList[i])
        {
            CanbusRfid *tmpRfidDevice=new CanbusRfid(xmlFilelist[i],aliasList[i], canbusIdList[i],
                                                     deviceIdList[i],canDriverIn);
            if(true!=tmpRfidDevice->isInitialOkStatus())
            {
                addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1943, -1,aliasList[i] );
                return -1;
            }
            rfidDriver.append(tmpRfidDevice);
            rfidDriverMap.insert(aliasList[i],tmpRfidDevice);
        }
        else if(E_EcatDev_maganeticSensor==deviceTypeList[i])
        {
            CanbusMaganetic *tmpCanbusMaganetic=new CanbusMaganetic(xmlFilelist[i],aliasList[i], canbusIdList[i],
                                                                    deviceIdList[i],canDriverIn);
            if(true!=tmpCanbusMaganetic->isInitialOkStatus())
            {
                addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1944, -1,aliasList[i] );
                return -1;
            }
            maganeticDriver.append(tmpCanbusMaganetic);
            maganeticDriverMap.insert(aliasList[i],tmpCanbusMaganetic);
        }
        else if(E_EcatDev_batterySensor==deviceTypeList[i])
        {
            CanbusBattery *tmpCanbusBattery=new CanbusBattery(xmlFilelist[i],aliasList[i], canbusIdList[i],
                                                                    deviceIdList[i],canDriverIn);
            if(true!=tmpCanbusBattery->isInitialOkStatus())
            {
                addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1945, -1,aliasList[i] );
                return -1;
            }
            batteryDriver.append(tmpCanbusBattery);
            batteryDriverMap.insert(aliasList[i],tmpCanbusBattery);
        }
        else if(E_EcatDev_IoModule==deviceTypeList[i])
        {
            CanIoDriver *tmpIoDriver=new CanIoDriver(xmlFilelist[i],aliasList[i], canbusIdList[i],
                                                     deviceIdList[i],canDriverIn);
            if(true!=tmpIoDriver->isInitialOkStatus())
            {
                addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1946, -1,aliasList[i] );
                return -1;
            }
            ioDriver.append(tmpIoDriver);
            ioDriverMap.insert(aliasList[i],tmpIoDriver);
        }


    }

    return 1;

}

void CanbusCom::masterError()
{
    QThread::msleep(100);
    RobotBase* rob;
    foreach( rob, robCtl->robotsMap )
    {
        if( !rob )
        {
            continue;
        }
//        if(eServoFaultReset==rob->canopenAxis.at(0)->motorStateCommand)
//        {
//            modbusResetStatus();
//        }

    }

}



void CanbusCom::masterInit()
{


}

void CanbusCom::masterPreOp()
{
    isFristTimeInOp=0;
//    int alisOut,indexOut;
    downloadMaganeticSdoConfig();
    downloadRfidSdoConfig();
    downloadServoSdoConfig();
    updateControlMode();
    setDriverNodeProtectTime();
    startPdoMode();//目前仅仅发送伺服的命令，ＩＯ等其它设备没有发送命令。
    initialMotorEncoderValue();
}

void CanbusCom::masterOp()
{
    if(1<=isFristTimeInOp)
    {
        TimeTool::getInstance()->recordPeirodTime(timer_id_measure[D_TIME_TIMER]);

    }


    if(false==isInitialedOk)
    {
//        if(-1==startPdoMode())
//        {
//            return ;
//        }
        sleep(2);
        isInitialedOk=true;
        isSlavesOp = true;
        addMsg(ENUM_MSG_REMIND, "EcatMaster", "CanbusCom" , 1900, -1 );

      }


    #if D_IF_DEBUG

    TimeTool::getInstance()->recordInTime(timer_id_measure[D_TIME_CUSTOM]);
    #endif
    if(true==isInitialedOk)
    {
        updateDomainData_pdo(); //读写过程数据 (帧数据交互)
//        updateDomainData_sdo(); //读写过程数据 (帧数据交互)
//        updateLogicData();
    }
    #if D_IF_DEBUG
    TimeTool::getInstance()->recordOutTime(timer_id_measure[D_TIME_CUSTOM]);

    #endif

    if(2==isFristTimeInOp)
    {
        checkTimeMaxLimit();
    }
    if(isFristTimeInOp<2)
    {
        isFristTimeInOp++;
    }
    opLoopCount++;
}




int CanbusCom::checkTimeMaxLimit()
{

    S_TIME_DATA* dataReceive;
    TimeTool::getInstance()->getTimeData(timer_id_measure[D_TIME_RECEIVE],&dataReceive);
    S_TIME_DATA* dataCustom ;
    TimeTool::getInstance()->getTimeData(timer_id_measure[D_TIME_CUSTOM],&dataCustom);
    S_TIME_DATA* dataSend ;
    TimeTool::getInstance()->getTimeData(timer_id_measure[D_TIME_SEND],&dataSend);
    S_TIME_DATA* dataPeriodTimer ;
    TimeTool::getInstance()->getTimeData(timer_id_measure[D_TIME_TIMER],&dataPeriodTimer);

    int a, b, c,e,f,g;
    e=timer_id_measure[D_TIME_RECEIVE];
    f=timer_id_measure[D_TIME_CUSTOM];
    g=timer_id_measure[D_TIME_SEND];
    a=dataReceive->maxTime;
    b=dataSend->maxTime;
    c=dataCustom->maxTime;

    ecatPeriodTimerMonitor_now=dataPeriodTimer->nowTime;
    ecatPeriodTimerMonitor_max=dataPeriodTimer->maxTime;
    ecatPeriodTimerMonitor_min=dataPeriodTimer->minTime;
    ecatReceiveMonitor_now=dataReceive->nowTime;
    ecatReceiveMonitor_max=dataReceive->maxTime;
    ecatReceiveMonitor_min=dataReceive->minTime;
    ecatSendMonitor_now=dataSend->nowTime;
    ecatSendMonitor_max=dataSend->maxTime;
    ecatSendMonitor_min=dataSend->minTime;
    ecatCustomMonitor_now=dataCustom->nowTime;
    ecatCustomMonitor_max=dataCustom->maxTime;
    ecatCustomMonitor_min=dataCustom->minTime;



    if(fabs(dataPeriodTimer->nowTime)>cycle_ns+ecatPeriodTimerOffsetMax)//300us
    {
//        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 9004, -1, dataPeriodTimer->nowTime
//               ,cycle_ns+ecatPeriodTimerOffsetMax,cycle_ns,cycle_us);
        timerErrorCount++;

    }
    else if(fabs(dataPeriodTimer->nowTime)<cycle_ns-ecatPeriodTimerOffsetMax)//300us
    {
//        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 9003, -1, dataPeriodTimer->nowTime
//               ,cycle_ns-ecatPeriodTimerOffsetMax,cycle_ns,cycle_us);
        timerErrorCount++;

    }
    else
    {
        timerErrorCount=0;
    }
    if(timerErrorCount>6)
    {
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1933, -1, dataPeriodTimer->nowTime
                       ,cycle_ns+ecatPeriodTimerOffsetMax,cycle_ns,cycle_us);
    }


    if(dataReceive->nowTime>ecatReceiveMax)//300us
    {
        receiveErrorCount++;
//        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 9302, -1, dataReceive->nowTime,ecatReceiveMax );

    }
    else
    {
        receiveErrorCount=0;
    }
    if(receiveErrorCount>4)
    {
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1934, -1, dataReceive->nowTime,ecatReceiveMax );
        resetMasterStatus();
    }

    if(dataSend->nowTime>ecatSendMax)//300us
    {
//        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 9303, -1, dataSend->nowTime,ecatSendMax );
        sendErrorCount++;
    }
    else
    {
        sendErrorCount=0;
    }
    if(sendErrorCount>4)
    {
       addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1935, -1, dataSend->nowTime,ecatSendMax );
       resetMasterStatus();
    }

    //if(dataCustom->maxTime>800000)//800us
    if(dataCustom->nowTime >  ecatCustomMax)//超过80%报警
    {
        customErrorCount++;
//        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 9304, -1, dataCustom->nowTime, ecatCustomMax);
//        printf("error : ethercat custom time exceed limit------------------------------");
    }
    else
    {
        customErrorCount=0;
    }
    if(customErrorCount>4)
    {
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1936, -1, dataCustom->nowTime, ecatCustomMax);
        resetMasterStatus();
    }

    return 1;
}

int CanbusCom::updateControlMode()
{
    int tmpReturn;
    int tryCount=0;
    RobotBase* rob;
    foreach( rob, robCtl->robotsMap )
    {
        if( !rob )
        {
            continue;
        }

//            int tmpKey=writeMotorData_modbus_rtu(rob);
            if(robCtl->vrepCtl->motorSmiulationMode!=E_ECAT_VREP )
            {

                for( int i=0;i<rob->canopenAxis.size();i++ )
                {
                    do
                    {
                        if(false==motorDriverMap.contains(rob->canopenAxis.at(i)->alias))
                        {
                            setMasterState(eMasterErr_LOCK);
                            return -1;
                        }

                        tmpReturn=motorDriverMap.value(rob->canopenAxis.at(i)->alias)
                                ->setDriverMode(rob->canopenAxis.at(i)->logicMotors[0]->getControlMode());

                        tryCount++;
                        if(1==tmpReturn)
                        {
                            break;
                        }
                        if(tryCount>15)
                        {
                            addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1913, -1 ,rob->canopenAxis.at(i)->alias);
                            setMasterState(eMasterErr_LOCK);
                            return -1;
                        }
                        sleep(1);
                    }
                    while(1);

                }
            }

    }
    return 1;
}

int CanbusCom::setDriverNodeProtectTime()
{
    int tmpReturn;
    int tryCount=0;
    RobotBase* rob;
    foreach( rob, robCtl->robotsMap )
    {
        if( !rob )
        {
            continue;
        }

//            int tmpKey=writeMotorData_modbus_rtu(rob);
            if(robCtl->vrepCtl->motorSmiulationMode!=E_ECAT_VREP )
            {

                for( int i=0;i<rob->canopenAxis.size();i++ )
                {
                    if(false==motorDriverMap.contains(rob->canopenAxis.at(i)->alias))
                    {
                        setMasterState(eMasterErr_LOCK);
                        return -1;
                    }


                        tmpReturn=motorDriverMap.value(rob->canopenAxis.at(i)->alias)
                                ->setDriverNodeProtectTime(7000);//


                }
            }

    }
    return 1;
}


int CanbusCom::writeMotorData_sdo(RobotBase* rob)
{
    int dataIndex=-1;
    if(robCtl->vrepCtl->motorSmiulationMode!=E_ECAT_VREP )
    {
        int   sdoReturnNum;
        int tmpErrorKey;
        bool errorFlag=false;

        for( int i=0;i<rob->canopenAxis.size();i++ )
        {
            //AGV 可以不更新位置
//            sdoReturnNum=motorDriverMap.value(rob->canopenAxis.at(i)->alias)
//                    ->setPosition(rob->canopenAxis.at(i)->targetPosition,true);
//            if(1!=sdoReturnNum)
//            {
//                tmpErrorKey=sdoReturnNum;
//                dataIndex=1;
//                errorFlag=true;
//            }

            //rob->canopenAxis.at(i)->offset.offsetTargetTorqueSet
//            if(1!=motorDriverMap.value(rob->canopenAxis.at(i)->alias)
//                    ->setMaxCurrency(rob->canopenAxis.at(i)->targetTorque))
//            {
//                errorFlag=true;
//            }

            sdoReturnNum=motorDriverMap.value(rob->canopenAxis.at(i)->alias)
                    ->setSpeed(rob->canopenAxis.at(i)->logicMotors[0]->targetVelocity,true);
            if(1!=sdoReturnNum)
            {
                tmpErrorKey=sdoReturnNum;
                dataIndex=2;
                errorFlag=true;
            }

            sdoReturnNum=motorDriverMap.value(rob->canopenAxis.at(i)->alias)
                    ->setControlWord(rob->canopenAxis.at(i)->logicMotors[0]->controlWord,true);
            if(1!=sdoReturnNum)
            {
                tmpErrorKey=sdoReturnNum;
                dataIndex=3;
                errorFlag=true;
            }

//            rob->canopenAxis.at(i)->targetVelocity
//            rob->canopenAxis.at(i)->offset.offsetTorqueOffSet
//            rob->canopenAxis.at(i)->torqueOffset
//            rob->canopenAxis.at(i)->offset.offsetSpeedOffSet
//            rob->canopenAxis.at(i)->velocityOffset
            if(true==errorFlag)
            {
//                addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1911, -1,
//                       rob->canopenAxis.at(i)->alias ,dataIndex,tmpErrorKey);
//                setMasterState(eMasterErr);
                return -1;
            }



        }


    }
    else
    {
        //read vrep motor data

    }



    return 1;
}

int CanbusCom::writeMotorDataByPdo(RobotBase *rob)
{
    if(robCtl->vrepCtl->motorSmiulationMode!=E_ECAT_VREP )
    {
        int   tmpKey;

        for( int i=0;i<rob->canopenAxis.size();i++ )
        {
            if(E_VENDOR_ID_ROBOTIQ2MOTOR==rob->canopenAxis.at(i)->vendorId
                    || E_VENDOR_ID_XingSong2MOTOR==rob->canopenAxis.at(i)->vendorId
                    || E_VENDOR_ID_CURTIS2MOTOR==rob->canopenAxis.at(i)->vendorId)
            {
                tmpKey=motorDriverMap.value(rob->canopenAxis.at(i)->alias)
                        ->sendPdoData_twoMotor(rob->canopenAxis.at(i)->logicMotors[0]->controlWord,
                                      rob->canopenAxis.at(i)->logicMotors[0]->targetPosition,
                                      rob->canopenAxis.at(i)->logicMotors[0]->targetVelocity,
                                       rob->canopenAxis.at(i)->logicMotors[0]->targetTorque,
                        rob->canopenAxis.at(i)->logicMotors[0]->pulsePerRound,
                                             rob->canopenAxis.at(i+1)->logicMotors[0]->controlWord,
                                             rob->canopenAxis.at(i+1)->logicMotors[0]->targetPosition,
                                             rob->canopenAxis.at(i+1)->logicMotors[0]->targetVelocity,
                                             rob->canopenAxis.at(i+1)->logicMotors[0]->targetTorque,
                        rob->canopenAxis.at(i+1)->logicMotors[0]->pulsePerRound);
                        i++ ;
            }
            else
            {
                tmpKey=motorDriverMap.value(rob->canopenAxis.at(i)->alias)
                        ->sendPdoData(rob->canopenAxis.at(i)->logicMotors[0]->controlWord,
                                      rob->canopenAxis.at(i)->logicMotors[0]->targetPosition,
                                      rob->canopenAxis.at(i)->logicMotors[0]->targetVelocity,
                                       rob->canopenAxis.at(i)->logicMotors[0]->targetTorque,
                                      rob->canopenAxis.at(i)->logicMotors[0]->pulsePerRound);
            }


            if(1!=tmpKey)
            {
//                addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1931, -1);
//                setMasterState(eMasterErr);
                return -1;
            }

        }


    }
    else
    {
        //read vrep motor data

    }


    return 1;
}




int CanbusCom::readMotorData_sdo(RobotBase* rob)
{
    int dataIndex=-1;
    if(robCtl->vrepCtl->motorSmiulationMode!=E_ECAT_VREP )
    {
        bool errorFlag=false;
        int sdoReturnNum;
        int tmpErrorKey;

        for( int i=0;i<rob->canopenAxis.size();i++ )
        {
                int tmpReturn=0;
//                motorDriverMap.value(rob->canopenAxis.at(i)->alias)->sendSdoGetCommandFrame();
//                tmpReturn=motorDriverMap.value(rob->canopenAxis.at(i)->alias)->process_can_buffer();
//                if(1!=tmpReturn)
//                {
//                    qDebug()<<"readMotorData process error, tmpReturn="<<tmpReturn;
//                    return -1;
//                }

                sdoReturnNum=motorDriverMap.value(rob->canopenAxis.at(i)->alias)->getErrorCode(tmpReturn);
                if(1==sdoReturnNum)
                {
                    rob->canopenAxis.at(i)->logicMotors[0]->errorCode=tmpReturn;

                }
                else
                {
                    tmpErrorKey=sdoReturnNum;
                    dataIndex=1;
                    errorFlag=true;

                }
                //AGV可以不更新位置反馈
//                sdoReturnNum=motorDriverMap.value(rob->canopenAxis.at(i)->alias)->getPositionFeedback(tmpReturn);
//                if(1==sdoReturnNum)
//                {
//                rob->canopenAxis.at(i)->actPosition = tmpReturn ;
//                rob->canopenAxis.at(i)->encoderValue = rob->canopenAxis.at(i)->actPosition ;
//                }
//                else
//                {
//                    tmpErrorKey=sdoReturnNum;
//                    dataIndex=2;
//                     errorFlag=true;
//                }

                double speed;
                sdoReturnNum=motorDriverMap.value(rob->canopenAxis.at(i)->alias)->getSpeedFeedback(speed);
                if(1==sdoReturnNum)
                {
                    rob->canopenAxis.at(i)->logicMotors[0]->writeActVelocity ( speed ) ;
                }
                else
                {
                    tmpErrorKey=sdoReturnNum;
                    dataIndex=3;
                     errorFlag=true;
                }
                double currency;
                sdoReturnNum=motorDriverMap.value(rob->canopenAxis.at(i)->alias)->getCurrencyFeedback(currency);
                if(1==sdoReturnNum)
                {
                   rob->canopenAxis.at(i)->logicMotors[0]->actTorque = currency ;
                }
                else
                {
                    tmpErrorKey=sdoReturnNum;
                    dataIndex=4;
                     errorFlag=true;
                }

                sdoReturnNum=motorDriverMap.value(rob->canopenAxis.at(i)->alias)->getStatusWord(tmpReturn);
                if(1==sdoReturnNum)
                {
                    rob->canopenAxis.at(i)->logicMotors[0]->stateWord= tmpReturn ;
                }
                else
                {
                    tmpErrorKey=sdoReturnNum;
                    dataIndex=5;
                     errorFlag=true;
                }

                //rob->canopenAxis.at(i)->actFollowErr =  ;
                //rob->canopenAxis.at(i)->disModeOfOperation = ;
                //rob->canopenAxis.at(i)->digIn = ;
                //rob->canopenAxis.at(i)->digOut =  ;

                if(true==errorFlag)
                {
//                    addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1906, -1,
//                           rob->canopenAxis.at(i)->alias,dataIndex,tmpErrorKey);
//                    setMasterState(eMasterErr);
                    return -1;
                }

        }


    }
    else
    {
        //read vrep motor data
        return -100;

    }

    return 1;
}

int CanbusCom::readMotorDataByPdo(RobotBase *rob, int &aliasOut)
{


    if(robCtl->vrepCtl->motorSmiulationMode!=E_ECAT_VREP )
    {
        int goodSize=0;
        for( int i=0;i<rob->canopenAxis.size();i++ )
        {

                int tmpReturn=-1;
                tmpReturn=motorDriverMap.value(rob->canopenAxis.at(i)->alias)->process_can_buffer_pdo();
                if(E_VENDOR_ID_ROBOTIQ2MOTOR==rob->canopenAxis.at(i)->vendorId
                        || E_VENDOR_ID_XingSong2MOTOR==rob->canopenAxis.at(i)->vendorId
                        || E_VENDOR_ID_CURTIS2MOTOR==rob->canopenAxis.at(i)->vendorId)
                {
                    if(1==tmpReturn)
                    {
                        goodSize++;
                        goodSize++;
                    }
                    else
                    {
                        aliasOut=rob->canopenAxis.at(i)->alias;
                    }
                    unsigned int tmpErrorCode;
                    motorDriverMap.value(rob->canopenAxis.at(i)->alias)->getErrorCode_buffer(tmpErrorCode);
                    rob->canopenAxis.at(i)->logicMotors[0]->errorCode=tmpErrorCode;
                    motorDriverMap.value(rob->canopenAxis.at(i)->alias)->getErrorCode_buffer_channel2(tmpErrorCode);
                    rob->canopenAxis.at(i+1)->logicMotors[0]->errorCode=tmpErrorCode;

                    //AGV可以不更新位置反馈
                    motorDriverMap.value(rob->canopenAxis.at(i)->alias)->getPositionFeedback_buffer(tmpReturn);
                    rob->canopenAxis.at(i)->logicMotors[0]->actPosition = tmpReturn ;
                    rob->canopenAxis.at(i)->logicMotors[0]->encoderValue = rob->canopenAxis.at(i)->logicMotors[0]->actPosition ;
                    motorDriverMap.value(rob->canopenAxis.at(i)->alias)->getPositionFeedback_buffer_channel2(tmpReturn);
                    rob->canopenAxis.at(i+1)->logicMotors[0]->actPosition= tmpReturn ;
                    rob->canopenAxis.at(i+1)->logicMotors[0]->encoderValue = rob->canopenAxis.at(i+1)->logicMotors[0]->actPosition ;


                    double speed;
                    motorDriverMap.value(rob->canopenAxis.at(i)->alias)->getSpeedFeedback_buffer(speed);
                    rob->canopenAxis.at(i)->logicMotors[0]->writeActVelocity ( speed) ;
                    motorDriverMap.value(rob->canopenAxis.at(i)->alias)->getSpeedFeedback_buffer_channel2(speed);
                    rob->canopenAxis.at(i+1)->logicMotors[0]->writeActVelocity ( speed ) ;

                    double currency;
                    motorDriverMap.value(rob->canopenAxis.at(i)->alias)->getCurrencyFeedback_buffer(currency);
                    rob->canopenAxis.at(i)->logicMotors[0]->actTorque = currency ;
                    motorDriverMap.value(rob->canopenAxis.at(i)->alias)->getCurrencyFeedback_buffer_channel2(currency);
                    rob->canopenAxis.at(i+1)->logicMotors[0]->actTorque = currency ;

                    motorDriverMap.value(rob->canopenAxis.at(i)->alias)->getStatusWord_buffer(tmpReturn);
                    rob->canopenAxis.at(i)->logicMotors[0]->stateWord = tmpReturn ;
                    motorDriverMap.value(rob->canopenAxis.at(i)->alias)->getStatusWord_buffer_channel2(tmpReturn);
                    rob->canopenAxis.at(i+1)->logicMotors[0]->stateWord= tmpReturn ;


                    //rob->canopenAxis.at(i)->actFollowErr =  ;
                    //rob->canopenAxis.at(i)->disModeOfOperation = ;
                    //rob->canopenAxis.at(i)->digIn = ;
                    //rob->canopenAxis.at(i)->digOut =  ;

                    i++ ;
                }
                else
                {
                    if(1==tmpReturn)
                    {
                        goodSize++;
                    }
                    else
                    {
                        aliasOut=rob->canopenAxis.at(i)->alias;
                    }
                    unsigned int tmpErrorCode;
                    motorDriverMap.value(rob->canopenAxis.at(i)->alias)->getErrorCode_buffer(tmpErrorCode);
                    rob->canopenAxis.at(i)->logicMotors[0]->errorCode=tmpErrorCode;

                    //AGV可以不更新位置反馈
                    motorDriverMap.value(rob->canopenAxis.at(i)->alias)->getPositionFeedback_buffer(tmpReturn);
                    rob->canopenAxis.at(i)->logicMotors[0]->actPosition = tmpReturn ;
                    rob->canopenAxis.at(i)->logicMotors[0]->encoderValue = rob->canopenAxis.at(i)->logicMotors[0]->actPosition ;


                    double speed;
                    motorDriverMap.value(rob->canopenAxis.at(i)->alias)->getSpeedFeedback_buffer(speed);
                    rob->canopenAxis.at(i)->logicMotors[0]->writeActVelocity ( speed) ;

                    double currency;
                    motorDriverMap.value(rob->canopenAxis.at(i)->alias)->getCurrencyFeedback_buffer(currency);
                    rob->canopenAxis.at(i)->logicMotors[0]->actTorque = currency ;

                    motorDriverMap.value(rob->canopenAxis.at(i)->alias)->getStatusWord_buffer(tmpReturn);
                    rob->canopenAxis.at(i)->logicMotors[0]->stateWord = tmpReturn ;


                    //rob->canopenAxis.at(i)->actFollowErr =  ;
                    //rob->canopenAxis.at(i)->disModeOfOperation = ;
                    //rob->canopenAxis.at(i)->digIn = ;
                    //rob->canopenAxis.at(i)->digOut =  ;
                }


        }
        if(goodSize==rob->canopenAxis.size())
        {
            return 1;
        }
        else
        {
//            qDebug()<<"good pdo motor size="<<goodSize;
            return -1;
        }


    }
    else
    {
        //read vrep motor data
        return -100;

    }


    return 1;
}




int CanbusCom::readIoData(RobotBase *rob,int &aliasOut)
{
    if(robCtl->vrepCtl->ioSmiulationMode!=E_ECAT_VREP )
   {

       //read real io_module data
       for( int i=0;i<rob->canopenIoModule.size();i++ )
       {

           QVector<bool> tmpDi;
           int tmpKey=ioDriverMap.value(rob->canopenIoModule[i]->alias)->updateData();
           if(1!=tmpKey)
           {
               aliasOut=rob->canopenIoModule[i]->alias;
               return -1;
//               addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1707, -1,tmpKey );

           }
           ioDriverMap.value(rob->canopenIoModule[i]->alias)->readAllDi(tmpDi);
           for(int j=0;j<tmpDi.size();j++)
           {
                rob->canopenIoModule[i]->can_diMemory[j]=tmpDi[j];
           }

           QVector<double> tmpAi;
           ioDriverMap.value(rob->canopenIoModule[i]->alias)->readAllAi(tmpAi);
           for(int j=0;j<tmpAi.size();j++)
           {
                rob->canopenIoModule[i]->can_aiMemory[j]=tmpAi[j];
           }
       }

   }
   else
   {
       //read vrep io
       // todo
   }
    return 1;
}





int CanbusCom::writeIoData(RobotBase *rob)
{
    if(robCtl->vrepCtl->ioSmiulationMode!=E_ECAT_VREP )
   {

       //write real io_module data
       for( int i=0;i<rob->canopenIoModule.size();i++ )
       {

           QVector<bool> tmpDo;
           int doSize=rob->canopenIoModule[i]->doCount;
           tmpDo.resize(doSize);
           for(int k=0;k<rob->canopenIoModule[i]->doCount;k++)
           {
               tmpDo[k]=rob->canopenIoModule[i]->can_doMemory[k];
           }

           QVector<double> tmpAo;
           int aoSize=rob->canopenIoModule[i]->aoCount;
           tmpAo.resize(aoSize);
           for(int k=0;k<rob->canopenIoModule[i]->aoCount;k++)
           {
               tmpAo[k]=rob->canopenIoModule[i]->can_aoMemory[k];
           }

           int tmpKey=ioDriverMap.value(rob->canopenIoModule[i]->alias)->writeDo(tmpDo,tmpAo);
           if(1!=tmpKey)
           {
               return -1;
//               addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1710, -1,tmpKey );
           }

       }

   }
   else
   {
       //write vrep io
       // todo
        return -100;
   }
    return 1;
}

int CanbusCom::readRfidData(int &aliasOut)
{

    if(robCtl->vrepCtl->motorSmiulationMode!=E_ECAT_VREP )
    {
        bool errorFlag=false;
        int tmpKey;

        for( int i=0;i<rfidDriver.size();i++ )
        {

                tmpKey=rfidDriver[i]->updateData();
                if(1==tmpKey)
                {

                }
                else
                {
                    errorFlag=true;

                }

                if(true==errorFlag)
                {
                    int busNumberOut;
                    int deviceIdOut;
                    rfidDriver[i]->getDeviceAddressInfo(aliasOut,busNumberOut,deviceIdOut);
//                    addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1917, -1,
//                           aliasOut,busNumberOut,deviceIdOut,tmpKey );
//                    setMasterState(eMasterErr);
                    return -1;
                }

        }



    }
    else
    {
        //read vrep motor data
        return -100;

    }

    return 1;
}

int CanbusCom::sendRfidDataRequestCommand()
{
    if(robCtl->vrepCtl->motorSmiulationMode!=E_ECAT_VREP )
    {
        bool errorFlag=false;
        int tmpKey;

        for( int i=0;i<rfidDriver.size();i++ )
        {

                tmpKey=rfidDriver[i]->sendDataRequestCommand();
                if(1==tmpKey)
                {

                }
                else
                {
                    errorFlag=true;

                }

                if(true==errorFlag)
                {
                    int aliasOut;
                    int busNumberOut;
                    int deviceIdOut;
                    rfidDriver[i]->getDeviceAddressInfo(aliasOut,busNumberOut,deviceIdOut);
//                    addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1917, -1,
//                           aliasOut,busNumberOut,deviceIdOut,tmpKey );
//                    setMasterState(eMasterErr);
                    return -1;
                }

        }



    }
    else
    {
        //read vrep motor data
        return -100;

    }

    return 1;
}

int CanbusCom::writeRfidData()
{

}

int CanbusCom::readMaganeticData(int &aliasOut)
{

    if(robCtl->vrepCtl->motorSmiulationMode!=E_ECAT_VREP )
    {
        bool errorFlag=false;
        int tmpKey;

        for( int i=0;i<maganeticDriver.size();i++ )
        {
                tmpKey=maganeticDriver[i]->updateData();
                if(1==tmpKey)
                {

                }
                else
                {
                    errorFlag=true;

                }

                if(true==errorFlag)
                {
                    int busNumberOut;
                    int deviceIdOut;
                    maganeticDriver[i]->getDeviceAddressInfo(aliasOut,busNumberOut,deviceIdOut);
//                    addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1918, -1,
//                           aliasOut,busNumberOut,deviceIdOut,tmpKey );
//                    setMasterState(eMasterErr);
                    return -1;
                }

        }



    }
    else
    {
        //read vrep motor data
        return -100;

    }

    return 1;
}

int CanbusCom::readBatteryData(int &aliasOut)
{
    if(robCtl->vrepCtl->motorSmiulationMode!=E_ECAT_VREP )
    {
        bool errorFlag=false;
        int tmpKey;

        for( int i=0;i<batteryDriver.size();i++ )
        {
                tmpKey=batteryDriver[i]->updateData();
                if(1==tmpKey)
                {

                }
                else
                {
                    errorFlag=true;

                }

                if(true==errorFlag)
                {
                    int busNumberOut;
                    int deviceIdOut;
                    batteryDriver[i]->getDeviceAddressInfo(aliasOut,busNumberOut,deviceIdOut);
//                    addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1918, -1,
//                           aliasOut,busNumberOut,deviceIdOut,tmpKey );
//                    setMasterState(eMasterErr);
                    return -1;
                }

        }



    }
    else
    {
        //read vrep motor data
        return -100;

    }

    return 1;
}

int CanbusCom::sendMaganeticDataRequestCommand()
{

    if(robCtl->vrepCtl->motorSmiulationMode!=E_ECAT_VREP )
    {
        bool errorFlag=false;
        int tmpKey;

        for( int i=0;i<maganeticDriver.size();i++ )
        {
                tmpKey=maganeticDriver[i]->sendMaganeticDataRequestCommand();
                if(1==tmpKey)
                {

                }
                else
                {
                    errorFlag=true;

                }

                if(true==errorFlag)
                {
                    return -1;
                }

        }



    }
    else
    {
        //read vrep motor data
        return -100;

    }

    return 1;
}

int CanbusCom::initialEcatIoDidoCount()
{
    int diNum,doNum,aiNum,aoNum;
    for(int i=0;i<ioDriver.size();i++)
    {
        ioDriver[i]->getDiDoCount(diNum,doNum,aiNum,aoNum);

        for(int j=0;j<robCtl->busManager->canopen_ioModule.size();j++)
        {
            if(robCtl->busManager->canopen_ioModule[j]->alias==ioDriver[i]->alias)
            {
                robCtl->busManager->canopen_ioModule[j]->setCount(diNum,doNum,aiNum,aoNum);
                break;
            }
        }


    }
    return 1;
}

int CanbusCom::createDeviceDataExchangeThread()
{
    int ret=pthread_create( &device_thId, NULL, &CanbusCom::threadTask_device, this);
    if(ret!=0)
    {
        printf("Create pthread printf!\n");
    }
    printf("\n\n--createDeviceDataExchangeThread thread start!--\n\n");
}

void* CanbusCom::threadTask_device( void* classPtr )
{
    return ((CanbusCom*)classPtr)->run_device(NULL);
}


void* CanbusCom::run_device(void *)
{
    printf("\n\n--run_device thread start!--\n\n");

    struct sched_param param;
    param.__sched_priority = 97;
    sched_setscheduler(0,SCHED_FIFO,&param);

    wakeup_time = system_time_ns() + 10 * cycle_ns;
//    while(isRun)
//    {
//        updateDeviceData();
//        wait_period_device();

//    }
    printf("Ethercat master had stopped222!");
}


/** Wait for the next period
 */
void CanbusCom::wait_period_device(void)
{
    while (1)
    {
        uint64_t wakeup_count = system2count(wakeup_time_device);
        uint64_t current_count = system_time_ns_no_plus_base();

        if ((wakeup_count < current_count)
                || (wakeup_count > current_count + (50 * cycle_ns))) {
//            printf("%s(): unexpected wake time!\n", __func__);
        }
        struct timespec tmpTimeSleep=ns2timespec(wakeup_count);
        clock_nanosleep(CLOCK_MONOTONIC ,TIMER_ABSTIME, &tmpTimeSleep, NULL);

//        switch (rt_sleep_until(wakeup_count)) {
//            case RTE_UNBLKD:
//                rt_printk("rt_sleep_until(): RTE_UNBLKD\n");
//                continue;

//            case RTE_TMROVRN:
//                rt_printk("rt_sleep_until(): RTE_TMROVRN\n");
//                overruns++;

//                if (overruns % 100 == 0) {
//                    // in case wake time is broken ensure other processes get
//                    // some time slice (and error messages can get displayed)
//                    rt_sleep(cycle_ns / 100);
//                }
//                break;

//            default:
//                break;
//        }

        // done if we got to here
        break;
    }

    // calc next wake time (in sys time)
    wakeup_time_device += cycle_ns;
}

int CanbusCom::controllerShutDown()
{
    setMasterState( eMasterSleep );
    stop();
    canbusDriver->closeDevice();
    qDebug()<<"can adapter closed";
    return 1;
}

int CanbusCom::sendNodeWatchDogFrame()
{
    RobotBase* rob;
    int tmpReturn;
    foreach( rob, robCtl->robotsMap )
    {
        if( !rob )
        {
            continue;
        }
            if(robCtl->vrepCtl->motorSmiulationMode!=E_ECAT_VREP )
            {

                for( int i=0;i<rob->canopenAxis.size();i++ )
                {

                        tmpReturn=motorDriverMap.value(rob->canopenAxis.at(i)->alias)
                                ->sendNodeWatchDogFrame();
                        if(1!=tmpReturn)
                        {
                            return -1;
                        }


                }
            }

    }
    return 1;
}

int CanbusCom::startPdoMode()
{
    int tmpReturn;
    int tryCount=0;
    RobotBase* rob;
    foreach( rob, robCtl->robotsMap )
    {
        if( !rob )
        {
            continue;
        }

//            int tmpKey=writeMotorData_modbus_rtu(rob);
            if(robCtl->vrepCtl->motorSmiulationMode!=E_ECAT_VREP )
            {
                //start motor
                tryCount=0;
                for( int i=0;i<rob->canopenAxis.size();i++ )
                {
                    do
                    {
                        if(false==motorDriverMap.contains(rob->canopenAxis.at(i)->alias))
                        {
                            setMasterState(eMasterErr_LOCK);
                            return -1;
                        }

                        tmpReturn=motorDriverMap.value(rob->canopenAxis.at(i)->alias)
                                ->startPdoMode();
                        tryCount++;
                        if(1==tmpReturn)
                        {
                            qDebug()<<"startPdoMode sucess, node "<<rob->canopenAxis.at(i)->alias;
                            break;
                        }
                        if(tryCount>15)
                        {
                            addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1928, -1 );
                            setMasterState(eMasterErr_LOCK);
                            return -1;
                        }
                        sleep(1);
                    }
                    while(1);

                }


                //start io
                tryCount=0;
                for( int i=0;i<rob->canopenIoModule.size();i++ )
                {
                    do
                    {
                        if(false==ioDriverMap.contains(rob->canopenIoModule.at(i)->alias))
                        {
                            setMasterState(eMasterErr_LOCK);
                            return -1;
                        }

                        tmpReturn=ioDriverMap.value(rob->canopenIoModule.at(i)->alias)
                                ->startPdoMode();
                        tryCount++;
                        if(1==tmpReturn)
                        {
                            qDebug()<<"startPdoMode sucess, node "<<rob->canopenIoModule.at(i)->alias;
                            break;
                        }
                        if(tryCount>15)
                        {
                            addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1950, -1 );
                            setMasterState(eMasterErr_LOCK);
                            return -1;
                        }
                        sleep(1);
                    }
                    while(1);

                }

                //start maganetic
                tryCount=0;
                for( int i=0;i<maganeticDriver.size();i++ )
                {
                    do
                    {

                        tmpReturn=maganeticDriver[i]->startPdoMode();
                        tryCount++;
                        if(1==tmpReturn)
                        {
                            qDebug()<<"startPdoMode sucess, node "<<maganeticDriver[i]->getAlias();
                            break;
                        }
                        if(tryCount>15)
                        {
                            addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1951, -1 );
                            setMasterState(eMasterErr_LOCK);
                            return -1;
                        }
                        sleep(1);
                    }
                    while(1);

                }

                //start rfid
                tryCount=0;
                for( int i=0;i<rfidDriver.size();i++ )
                {
                    do
                    {

                        tmpReturn=rfidDriver[i]->startPdoMode();
                        tryCount++;
                        if(1==tmpReturn)
                        {
                            qDebug()<<"startPdoMode sucess, node "<<rfidDriver[i]->getAlias();
                            break;
                        }
                        if(tryCount>15)
                        {
                            addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1952, -1 );
                            setMasterState(eMasterErr_LOCK);
                            return -1;
                        }
                        sleep(1);
                    }
                    while(1);

                }

            }

    }
    return 1;
}

int CanbusCom::initialMotorEncoderValue()
{
    RobotBase* rob;
    foreach( rob, robCtl->robotsMap )
    {
        if( !rob )
        {
            continue;
        }
        rob->initialMotorEncoderValue();


    }
    return 1;
}

int CanbusCom::downloadServoSdoConfig()
{
    int tmpReturn;

    RobotBase* rob;
    foreach( rob, robCtl->robotsMap )
    {
        if( !rob )
        {
            continue;
        }

//            int tmpKey=writeMotorData_modbus_rtu(rob);
            if(robCtl->vrepCtl->motorSmiulationMode!=E_ECAT_VREP )
            {

                for( int i=0;i<rob->canopenAxis.size();i++ )
                {
                        if(false==motorDriverMap.contains(rob->canopenAxis.at(i)->alias))
                        {
                            setMasterState(eMasterErr_LOCK);
                            return -1;
                        }

                        tmpReturn=motorDriverMap.value(rob->canopenAxis.at(i)->alias)
                                ->downloadServoSdoConfig();
                        if(1!=tmpReturn)
                        {
                            addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1947, -1 ,rob->canopenAxis.at(i)->alias);
                            setMasterState(eMasterErr_LOCK);
                            return -1;
                        }


                }
            }

    }
    return 1;
}

int CanbusCom::downloadMaganeticSdoConfig()
{
    int tmpReturn;

    RobotBase* rob;
    foreach( rob, robCtl->robotsMap )
    {
        if( !rob )
        {
            continue;
        }

//            int tmpKey=writeMotorData_modbus_rtu(rob);
            if(robCtl->vrepCtl->motorSmiulationMode!=E_ECAT_VREP )
            {

                for( int i=0;i<maganeticDriver.size();i++ )
                {

                        tmpReturn=maganeticDriver[i]->downloadSdoConfig();
                        if(1!=tmpReturn)
                        {
                            addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1948, -1 ,maganeticDriver[i]->getAlias());
                            setMasterState(eMasterErr_LOCK);
                            return -1;
                        }


                }
            }

    }
    return 1;
}

int CanbusCom::downloadRfidSdoConfig()
{
    int tmpReturn;

    RobotBase* rob;
    foreach( rob, robCtl->robotsMap )
    {
        if( !rob )
        {
            continue;
        }

//            int tmpKey=writeMotorData_modbus_rtu(rob);
            if(robCtl->vrepCtl->motorSmiulationMode!=E_ECAT_VREP )
            {

                for( int i=0;i<rfidDriver.size();i++ )
                {

                        tmpReturn=rfidDriver[i]->downloadSdoConfig();
                        if(1!=tmpReturn)
                        {
                            addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1949, -1 ,rfidDriver[i]->getAlias());
                            setMasterState(eMasterErr_LOCK);
                            return -1;
                        }


                }
            }

    }
    return 1;
}


//void CanbusCom::updateDomainData()
//{

//    RobotBase* rob;
//    foreach( rob, robCtl->robotsMap )
//    {
//        if( !rob )
//        {
//            continue;
//        }


//        static tryCount_readMotor=0;
//        static tryCount_writeMotor=0;
//        static tryCount_readRfid=0;
//        static tryCount_readMaganetic=0;

//        int keyReturn;
//        TimeTool::getInstance()->recordInTime(timer_id_measure[D_TIME_RECEIVE]);


//            keyReturn = readMotorData(rob);
//            if(keyReturn>0)
//            {
//                break;
//            }
//            tryCount++;

//        if(tryCount>0)
//        {
//            qDebug()<<"readMotorData tryCount="<<tryCount;
//        }

//        if(keyReturn<0)
//        {
//            qDebug()<<"error, canbus updateDomainData error!";
//            addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1926, -1);
//            return ;
//        }

//        readIoData(rob);

//        tryCount=0;
//        while(tryCount<D_TRY_MAX_COUNT)
//        {
//            keyReturn = readRfidData();
//            if(keyReturn>0)
//            {
//                break;
//            }
//            tryCount++;

//        }
//        if(tryCount>0)
//        {
//            qDebug()<<"readRfidData tryCount="<<tryCount;
//        }

//        if(keyReturn<0)
//        {
//            qDebug()<<"error, canbus updateDomainData error!";
//            addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1917, -1);
//            return ;
//        }



////        readRfidData();

//        tryCount=0;
//        while(tryCount<D_TRY_MAX_COUNT)
//        {
//            keyReturn = readMaganeticData();
//            if(keyReturn>0)
//            {
//                break;
//            }
//            tryCount++;

//        }
//        if(tryCount>0)
//        {
//            qDebug()<<"readMaganeticData tryCount="<<tryCount;
//        }
//        if(keyReturn<0)
//        {
//            qDebug()<<"error, canbus updateDomainData error!";
//            addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1918, -1);
//            return ;
//        }
//        tryCount=0;

////        readMaganeticData();
//        TimeTool::getInstance()->recordOutTime(timer_id_measure[D_TIME_RECEIVE]);


////        rob->updateInputModbusTcpIodata();//io数据映射交换
////        rob->updateOutputModbusTcpIodata();

//        TimeTool::getInstance()->recordInTime(timer_id_measure[D_TIME_MATH]);
//        setServoData(rob);//算法数据交换
//        TimeTool::getInstance()->recordOutTime(timer_id_measure[D_TIME_MATH]);

//        rob->checkCanopenDeviceError();
//        for( int i=0;i<rob->canopenAxis.size();i++ )
//        {
//            rob->canopenAxis.at(i)->updateMotorState();
//        }

//        TimeTool::getInstance()->recordInTime(timer_id_measure[D_TIME_SEND]);

////        writeMotorDataByPdo(rob);

//        while(tryCount<D_TRY_MAX_COUNT)
//        {
//            keyReturn = writeMotorData(rob);
//            if(keyReturn>0)
//            {
//                break;
//            }
//            tryCount++;

//        }
//        if(tryCount>0)
//        {
//            qDebug()<<"writeMotorData tryCount="<<tryCount;
//        }
//        if(keyReturn<0)
//        {
//            qDebug()<<"error, canbus updateDomainData error!";
//            addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1927, -1);
////            setMasterState(eMasterErr);
//            return ;
//        }
//        tryCount=0;

//        writeIoData(rob);
//        TimeTool::getInstance()->recordOutTime(timer_id_measure[D_TIME_SEND]);

//    }

////    canbusDriver->clearUserFrameBuffer();

////    canbusDriver->sendSyncFrame(currentDeviceId,currentCanbusId);
////    sendNodeWatchDogFrame();

//}





void CanbusCom::updateDomainData_pdo()
{
    static int updateDomainLoopCount=0;
    if(updateDomainLoopCount<1000000)
    {
        updateDomainLoopCount++;
    }

    canbusDriver->receiveData_toUserBuffer1(currentDeviceId,currentCanbusId);
    canbusDriver->receiveData_toUserBuffer2(currentDeviceId,currentCanbusId+1);//can电池使用

    int keyReturn;
    int tmpErrorAliasOut;

    //rfid
    static int tryCount_readRfid=0;
    keyReturn = readRfidData(tmpErrorAliasOut);
    if(1!=keyReturn)
    {
        tryCount_readRfid++;
    }
    else
    {
        tryCount_readRfid=0;
    }
    if(tryCount_readRfid>0)
    {
//        qDebug()<<"readRfidData tryCount_readRfid="<<tryCount_readRfid;
    }
    if(tryCount_readRfid>D_TRY_MAX_COUNT_RFID && updateDomainLoopCount>100)
    {
        qDebug()<<"error, canbus updateDomainData error!tryCount_readRfid>D_TRY_MAX_COUNT";
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1917, -1,
                           tmpErrorAliasOut,0,0,0 );
    }

    //maganetic
    static int tryCount_readMaganetic=0;
    keyReturn = readMaganeticData(tmpErrorAliasOut);
    if(1!=keyReturn)
    {
        tryCount_readMaganetic++;
    }
    else
    {
        tryCount_readMaganetic=0;
    }
    if(tryCount_readMaganetic>0)
    {
//        qDebug()<<"readMaganeticData tryCount_readMaganetic="<<tryCount_readMaganetic;
    }
    if(tryCount_readMaganetic>D_TRY_MAX_COUNT && updateDomainLoopCount>100)
    {
        qDebug()<<"error, canbus updateDomainData error!tryCount_readMaganetic>D_TRY_MAX_COUNT";
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1918, -1,
                           tmpErrorAliasOut,0,0,0 );
    }

    //battery
    static int tryCount_readBattery=0;
    keyReturn = readBatteryData(tmpErrorAliasOut);
    if(1!=keyReturn)
    {
        tryCount_readBattery++;
    }
    else
    {
        tryCount_readBattery=0;
    }
    if(tryCount_readBattery>0)
    {
//        qDebug()<<"readBatteryData tryCount_readBattery="<<tryCount_readBattery;
    }
    if(tryCount_readBattery>D_TRY_MAX_COUNT_BATTERY && updateDomainLoopCount>100)
    {
        qDebug()<<"error, canbus updateDomainData error!tryCount_readBattery>D_TRY_MAX_COUNT_BATTERY";
        addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1941, -1,
                           tmpErrorAliasOut,0,1,0 );
    }

    RobotBase* rob;
    foreach( rob, robCtl->robotsMap )
    {
        if( !rob )
        {
            continue;
        }


        TimeTool::getInstance()->recordInTime(timer_id_measure[D_TIME_RECEIVE]);



        static int tryCount_readMotor=0;
        keyReturn=readMotorDataByPdo(rob,tmpErrorAliasOut);
        if(1==keyReturn)
        {
            tryCount_readMotor=0;
        }
        else
        {
            tryCount_readMotor++;
//            qDebug()<<"error, tryCount_readMotor="<<tryCount_readMotor;
        }
        if(tryCount_readMotor>D_TRY_MAX_COUNT && updateDomainLoopCount>100)//程序刚启动时，会报错
        {
            qDebug()<<"error, canbus updateDomainData error!tryCount_readMotor>D_TRY_MAX_COUNT";
            addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1937, -1,tmpErrorAliasOut);
        }


        static int tryCount_readIo=0;
        keyReturn=readIoData(rob,tmpErrorAliasOut);
        if(1==keyReturn)
        {
            tryCount_readIo=0;
        }
        else
        {
            tryCount_readIo++;
            //qDebug()<<"error, tryCount_readIo="<<tryCount_readIo;
        }
        if(tryCount_readIo>D_TRY_MAX_COUNT_IOMODULE && updateDomainLoopCount>100)//初始地图位置报错。
        {
            qDebug()<<"error, canbus updateDomainData error!tryCount_readIo>D_TRY_MAX_COUNT";
            addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1939, -1,tmpErrorAliasOut);
        }

        TimeTool::getInstance()->recordOutTime(timer_id_measure[D_TIME_RECEIVE]);

        rob->updateCanIoData();//io数据映射交换

        TimeTool::getInstance()->recordInTime(timer_id_measure[D_TIME_MATH]);
        setServoData(rob);//算法数据交换
        TimeTool::getInstance()->recordOutTime(timer_id_measure[D_TIME_MATH]);

        rob->checkCanopenDeviceError();
        for( int i=0;i<rob->canopenAxis.size();i++ )
        {
            rob->canopenAxis.at(i)->updateMotorState();
        }

        TimeTool::getInstance()->recordInTime(timer_id_measure[D_TIME_SEND]);


        static int tryCount_writeMotor=0;
        keyReturn=writeMotorDataByPdo(rob);
        if(1==keyReturn)
        {
            tryCount_writeMotor=0;
        }
        else
        {
            tryCount_writeMotor++;
//            qDebug()<<"error, tryCount_writeMotor="<<tryCount_writeMotor;
        }
        if(tryCount_writeMotor>D_TRY_MAX_COUNT)
        {
            qDebug()<<"error, canbus updateDomainData error!tryCount_writeMotor>D_TRY_MAX_COUNT";
            addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1938, -1);
        }

        static int tryCount_writeIo=0;
        keyReturn=writeIoData(rob);
        if(1==keyReturn)
        {
            tryCount_writeIo=0;
        }
        else
        {
            tryCount_writeIo++;
//            qDebug()<<"error, tryCount_writeIo="<<tryCount_writeIo;
        }
        if(tryCount_writeIo>D_TRY_MAX_COUNT)
        {
            qDebug()<<"error, canbus updateDomainData error!tryCount_writeIo>D_TRY_MAX_COUNT";
            addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1940, -1);
        }

        TimeTool::getInstance()->recordOutTime(timer_id_measure[D_TIME_SEND]);

    }

    canbusDriver->clearUserFrameBuffer();

    sendRfidDataRequestCommand();
    sendMaganeticDataRequestCommand();

    canbusDriver->sendSyncFrame(currentDeviceId,currentCanbusId);
    sendNodeWatchDogFrame();

}


void CanbusCom::updateDomainData_sdo()
{

    int tmpAliasOut;
//    canbusDriver->receiveData_toUserBuffer(currentDeviceId,currentCanbusId);
    RobotBase* rob;
    foreach( rob, robCtl->robotsMap )
    {
        if( !rob )
        {
            continue;
        }
//        if( !rob->isLinkOk )
//            continue;

//        QThread::msleep(10);

        int tryCount=0;
        int keyReturn;
        TimeTool::getInstance()->recordInTime(timer_id_measure[D_TIME_RECEIVE]);


//        readMotorDataByPdo(rob);
//        static int tryCount_readMotorData=0;
//        keyReturn = readMotorData(rob);
//        if(keyReturn>0)
//        {
//            tryCount_readMotorData=0;
//        }
//        else
//        {
//            tryCount_readMotorData++;
//            qDebug()<<"readMotorData failed ,tryCount_readMotorData="<<tryCount_readMotorData;
//        }

//        if(tryCount_readMotorData>D_TRY_MAX_COUNT)
//        {
//            qDebug()<<"error, canbus updateDomainData error!";
//            addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1926, -1);
////            setMasterState(eMasterErr);
//            return ;
//        }

        tryCount=0;
        while(tryCount<D_TRY_MAX_COUNT)
        {
            keyReturn = readMotorData_sdo(rob);
            if(keyReturn>0)
            {
                break;
            }
            tryCount++;

        }
        if(tryCount>0)
        {
            qDebug()<<"readMotorData tryCount="<<tryCount;
        }

        if(keyReturn<0)
        {
            qDebug()<<"error, canbus updateDomainData error!";
            addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1926, -1);
            return ;
        }

//        readIoData(rob);

        tryCount=0;
        while(tryCount<D_TRY_MAX_COUNT)
        {
            keyReturn = readRfidData(tmpAliasOut);
            if(keyReturn>0)
            {
                break;
            }
            tryCount++;

        }
        if(tryCount>0)
        {
            qDebug()<<"readRfidData tryCount="<<tryCount;
        }

        if(keyReturn<0)
        {
            qDebug()<<"error, canbus updateDomainData error!";
            addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1917, -1,
                               tmpAliasOut,0,0,0 );
            return ;
        }



//        readRfidData();

        tryCount=0;
        while(tryCount<D_TRY_MAX_COUNT)
        {
            keyReturn = readMaganeticData(tmpAliasOut);
            if(keyReturn>0)
            {
                break;
            }
            tryCount++;

        }
        if(tryCount>0)
        {
            qDebug()<<"readMaganeticData tryCount="<<tryCount;
        }
        if(keyReturn<0)
        {
            qDebug()<<"error, canbus updateDomainData error!";
            addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1918, -1,
                           tmpAliasOut,0,0,0 );
            return ;
        }
        tryCount=0;

//        readMaganeticData();
        TimeTool::getInstance()->recordOutTime(timer_id_measure[D_TIME_RECEIVE]);


//        rob->updateInputModbusTcpIodata();//io数据映射交换
//        rob->updateOutputModbusTcpIodata();

        TimeTool::getInstance()->recordInTime(timer_id_measure[D_TIME_MATH]);
        setServoData(rob);//算法数据交换
        TimeTool::getInstance()->recordOutTime(timer_id_measure[D_TIME_MATH]);

        rob->checkCanopenDeviceError();
        for( int i=0;i<rob->canopenAxis.size();i++ )
        {
            rob->canopenAxis.at(i)->updateMotorState();
        }

        TimeTool::getInstance()->recordInTime(timer_id_measure[D_TIME_SEND]);

//        writeMotorDataByPdo(rob);

        while(tryCount<D_TRY_MAX_COUNT)
        {
            keyReturn = writeMotorData_sdo(rob);
            if(keyReturn>0)
            {
                break;
            }
            tryCount++;

        }
        if(tryCount>0)
        {
            qDebug()<<"writeMotorData tryCount="<<tryCount;
        }
        if(keyReturn<0)
        {
            qDebug()<<"error, canbus updateDomainData error!";
            addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1927, -1);
//            setMasterState(eMasterErr);
            return ;
        }
        tryCount=0;

        writeIoData(rob);
        TimeTool::getInstance()->recordOutTime(timer_id_measure[D_TIME_SEND]);

    }

//    canbusDriver->clearUserFrameBuffer();

//    canbusDriver->sendSyncFrame(currentDeviceId,currentCanbusId);
    sendNodeWatchDogFrame();

}


void CanbusCom::updateDeviceData()
{
    int tmpAliasOut;

    RobotBase* rob;
    foreach( rob, robCtl->robotsMap_onlyForCanbus )
    {
        if( !rob )
        {
            continue;
        }

        int tryCount=0;
        int keyReturn;
        TimeTool::getInstance()->recordInTime(timer_id_measure[D_TIME_RECEIVE]);

        tryCount=0;
        while(tryCount<D_TRY_MAX_COUNT)
        {
            keyReturn = readMotorData_sdo(rob);
            if(keyReturn>0)
            {
                break;
            }
            tryCount++;

        }
        if(tryCount>0)
        {
            qDebug()<<"readMotorData tryCount="<<tryCount;
        }

        if(keyReturn<0)
        {
            qDebug()<<"error, canbus updateDomainData error!";
            addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1926, -1);
            return ;
        }

//        readIoData(rob);

        tryCount=0;
        while(tryCount<D_TRY_MAX_COUNT)
        {
            keyReturn = readRfidData(tmpAliasOut);
            if(keyReturn>0)
            {
                break;
            }
            tryCount++;

        }
        if(tryCount>0)
        {
            qDebug()<<"readRfidData tryCount="<<tryCount;
        }

        if(keyReturn<0)
        {
            qDebug()<<"error, canbus updateDomainData error!keyReturn<0";
            addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1917, -1,
                               tmpAliasOut,0,0,0 );
            return ;
        }



        tryCount=0;
        while(tryCount<D_TRY_MAX_COUNT)
        {
            keyReturn = readMaganeticData(tmpAliasOut);
            if(keyReturn>0)
            {
                break;
            }
            tryCount++;

        }
        if(tryCount>0)
        {
            qDebug()<<"readMaganeticData tryCount="<<tryCount;
        }
        if(keyReturn<0)
        {
            qDebug()<<"error, canbus updateDomainData error!";
            addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1918, -1,
                           tmpAliasOut,0,0,0 );
            return ;
        }
        tryCount=0;

        TimeTool::getInstance()->recordOutTime(timer_id_measure[D_TIME_RECEIVE]);


        rob->checkCanopenDeviceError();
        for( int i=0;i<rob->canopenAxis.size();i++ )
        {
            rob->canopenAxis.at(i)->updateMotorState();
        }

        TimeTool::getInstance()->recordInTime(timer_id_measure[D_TIME_SEND]);


        while(tryCount<D_TRY_MAX_COUNT)
        {
            keyReturn = writeMotorData_sdo(rob);
            if(keyReturn>0)
            {
                break;
            }
            tryCount++;

        }
        if(tryCount>0)
        {
            qDebug()<<"writeMotorData tryCount="<<tryCount;
        }
        if(keyReturn<0)
        {
            qDebug()<<"error, canbus update device Data  error!";
            addMsg(ENUM_MSG_ERROR, "EcatMaster", "CanbusCom" , 1927, -1);
            return ;
        }
        tryCount=0;

        writeIoData(rob);
        TimeTool::getInstance()->recordOutTime(timer_id_measure[D_TIME_SEND]);

    }

}

void CanbusCom::updateLogicData()
{
    RobotBase* rob;
    foreach( rob, robCtl->robotsMap)
    {
        if( !rob )
        {
            continue;
        }

        TimeTool::getInstance()->recordInTime(timer_id_measure[D_TIME_MATH]);
        setServoData(rob);//算法数据交换
        TimeTool::getInstance()->recordOutTime(timer_id_measure[D_TIME_MATH]);

    }
}

void CanbusCom::realTimeSleep(long long umicoroSecond)
{
    struct timespec time;
    clock_gettime(CLOCK_REALTIME,&time);
    time.tv_nsec += umicoroSecond * 1000;
    while(time.tv_nsec >= NSEC_PER_SEC)
    {
        time.tv_nsec -= NSEC_PER_SEC;
        ++time.tv_sec;
    }
    clock_nanosleep(CLOCK_REALTIME,TIMER_ABSTIME,&time,NULL);

}

void CanbusCom::addMsg(int messageLevel,
                            std::string componentName,
                            std::string messageType,
                            int messageCode,
                            int robotId,
                            int parameter1 ,
                            int parameter2 ,
                            int parameter3 ,
                            int parameter4 )
{
//    struct timeval tv_begin ;
//    gettimeofday(&tv_begin, NULL);

    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotId;
    tmpMsg.messageCode = messageCode;
//    tmpMsg.time = tv_begin.tv_sec;

    QString infomationStr;
    switch(messageCode)
    {
    case 9065:
    {
        infomationStr =  QObject::tr("canopen主站处于错误锁死状态，请重启控制器!!!");
        break;

    }
    case 1952:
    {
        infomationStr =  QObject::tr("canopen rfid进入OP模式失败 !");
        break;

    }
    case 1951:
    {
        infomationStr =  QObject::tr("canopen maganetic进入OP模式失败 !");
        break;

    }
    case 1950:
    {
        infomationStr =  QObject::tr("canopen ioModule进入OP模式失败 !");
        break;

    }
    case 1949:
    {
        infomationStr =  QObject::tr("canbus rfid读卡器 sdo配置下载失败,设备别名%1 !").arg(parameter1);
        break;

    }
    case 1948:
    {
        infomationStr =  QObject::tr("canbus 磁传感器 sdo配置下载失败,设备别名%1 !").arg(parameter1);
        break;

    }
    case 1947:
    {
        infomationStr =  QObject::tr("canbus motor sdo配置下载失败,设备别名%1 !").arg(parameter1);
        break;

    }
    case 1946:
    {
        infomationStr =  QObject::tr("canIo模块初始化失败，nodeId=%1 !")
                .arg(parameter1);
        break;

    }
    case 1945:
    {
        infomationStr =  QObject::tr("can电池模块初始化失败，nodeId=%1 !")
                .arg(parameter1);
        break;

    }
    case 1944:
    {
        infomationStr =  QObject::tr("can磁传感器模块初始化失败，nodeId=%1 !")
                .arg(parameter1);
        break;

    }
    case 1943:
    {
        infomationStr =  QObject::tr("canRFID模块初始化失败，nodeId=%1 !")
                .arg(parameter1);
        break;

    }
    case 1942:
    {
        infomationStr =  QObject::tr("can电机模块初始化失败，nodeId=%1 !")
                .arg(parameter1);
        break;

    }
    case 1941:
    {
        infomationStr =  QObject::tr("can电池设备读取数据失败，nodeId=%1,busId=%2,canDeviceId=%3,sdoErrorFlag=%4 !")
                .arg(parameter1).arg(parameter2).arg(parameter3).arg(parameter4);
        break;

    }
    case 1940:
    {
        infomationStr =  QObject::tr("can io设备 写入失败 error !");
    }
        break;
    case 1939:
    {
        infomationStr =  QObject::tr("can io设备 读取失败,设备别名＝(%1) !").arg(parameter1);
    }
        break;
    case 1938:
    {
        infomationStr =  QObject::tr("can motor pdo write error !");
    }
        break;
    case 1937:
    {
        infomationStr =  QObject::tr("can motor pdo read error,电机别名＝(%1) !").arg(parameter1);
    }
        break;
    case 1936:
    {
        infomationStr =  QObject::tr("can custom time(%1) exceed limit(%2 us) !")
                .arg(parameter1/1000).arg(parameter2/1000);
    }
        break;
    case 1935:
    {
        infomationStr =  QObject::tr("can send time(%1) exceed limit(%2 us) !")
                .arg(parameter1/1000).arg(parameter2/1000);

    }
        break;
    case 1934:
    {
        infomationStr =  QObject::tr("can receive time(%1 us) exceed limit(%2 us) !")
                .arg(parameter1/1000).arg(parameter2/1000);
    }
        break;
    case 1933:
    {
        infomationStr =  QObject::tr("can period timer(%1 us) exceed  limit(%2 us),cycle_ns=%3,cycle_us=%4  !")
                .arg(parameter1/1000).arg(parameter2/1000).arg(parameter3/1000).arg(parameter4/1000);

    }
        break;

    case 1932:
    {
        infomationStr =  QObject::tr("can checkConfig.ini文件读取失败 !");
        break;

    }
    case 1931:
    {
        infomationStr =  QObject::tr("pdo发送失败 !");
        break;

    }
    case 1930:
    {
        infomationStr =  QObject::tr("sync信号发送失败 !");
        break;

    }
    case 1929:
    {
        infomationStr =  QObject::tr("pdo接收失败 !");
        break;

    }
    case 1928:
    {
        infomationStr =  QObject::tr("canbus motor进入OP模式失败 !");
        break;

    }
    case 1927:
    {
        infomationStr =  QObject::tr("电机数据写失败次数超过允许值!");
        break;

    }
    case 1926:
    {
        infomationStr =  QObject::tr("电机数据读取失败次数超过允许值!");
        break;

    }
    case 1925:
    {
        infomationStr =  QObject::tr("can收到的数据帧全部不是目标从站的数据帧!");
        break;

    }
    case 1924:
    {
        infomationStr =  QObject::tr("can从站回应的SDO数据帧状态码不识别!");
        break;

    }
    case 1923:
    {
        infomationStr =  QObject::tr("can从站回应SDO操作不成功!");
        break;

    }
    case 1922:
    {
        infomationStr =  QObject::tr("can总线SDO等待数据帧回应失败!");
        break;

    }
    case 1921:
    {
        infomationStr =  QObject::tr("can总线发送原始数据帧失败 !");
        break;

    }
    case 1920:
    {
        infomationStr =  QObject::tr("can总线sdo download失败 !");
        break;

    }
    case 1919:
    {
        infomationStr =  QObject::tr("can总线sdo upload失败 !");
        break;

    }
    case 1918:
    {
        infomationStr =  QObject::tr("磁条传感器设备读取数据失败，nodeId=%1,busId=%2,canDeviceId=%3,sdoErrorFlag=%4 !")
                .arg(parameter1).arg(parameter2).arg(parameter3).arg(parameter4);
        break;

    }
    case 1917:
    {
        infomationStr =  QObject::tr("rfid设备读取数据失败，nodeId=%1,busId=%2,canDeviceId=%3,sdoErrorFlag=%4 !")
                .arg(parameter1).arg(parameter2).arg(parameter3).arg(parameter4);
        break;

    }
    case 1916:
    {
        infomationStr =  QObject::tr("can总线接受数据失败 !");
        break;

    }
    case 1915:
    {
        infomationStr =  QObject::tr("can总线发送数据失败 !");
        break;

    }
    case 1914:
    {
        infomationStr =  QObject::tr("canbus motor设备操作模式设置失败 !");
        break;

    }
    case 1913:
    {
        infomationStr =  QObject::tr("canbus motor设备操作模式设置失败,设备别名%1 !").arg(parameter1);
        break;

    }
    case 1912:
    {
        infomationStr =  QObject::tr("canopen motor设备写操作失败，node=%1 !").arg(parameter1);
        break;

    }
    case 1911:
    {
        infomationStr =  QObject::tr("canbus motor设备写操作失败，alias=%1 ,dataIndex=%2,sdoReturnNum=%3!")
                .arg(parameter1).arg(parameter2).arg(parameter3);
        break;

    }
    case 1910:
    {
        infomationStr =  QObject::tr("canbus io写备读操作失败，returnKey=%1 !").arg(parameter1);
        break;

    }
    case 1909:
    {
        infomationStr =  QObject::tr("canbus io写备读操作失败，returnKey=%1 !").arg(parameter1);
        break;

    }
    case 1908:
    {
        infomationStr =  QObject::tr("canbus io设备读操作失败，returnKey=%1 !").arg(parameter1);
        break;

    }
    case 1907:
    {
        infomationStr =  QObject::tr("canbus io设备读操作失败，returnKey=%1 !").arg(parameter1);
        break;

    }
    case 1906:
    {
        infomationStr =  QObject::tr("canbus motor设备读操作失败，alias=%1 ,dataIndex=%2,sdoReturnNum=%3!")
                .arg(parameter1).arg(parameter2).arg(parameter3);
        break;

    }
    case 1905:
    {
        infomationStr =  QObject::tr("canbus motor设备读操作失败，returnKey=%1 !").arg(parameter1);
        break;

    }

    case 1904:
    {
        infomationStr =  QObject::tr("canbus 设备连接不上,%1号模块,busType=%2 !")
                .arg(parameter1).arg(parameter2);
        break;

    }
    case 1903:
    {
        infomationStr =  QObject::tr("canbus 设备配置文件错误,%1号模块,busType=%2 !")
                .arg(parameter1).arg(parameter2);
        break;

    }
    case 1902:
    {
        infomationStr =  QObject::tr("canbus 设备连接不上,%1号模块,busType=%2 !")
                .arg(parameter1).arg(parameter2);
        break;

    }
    case 1901:
    {
        infomationStr =  QObject::tr("canbus　错误，can驱动适配器没有正常开启 !");
        break;

    }

    case 1900:
    {
        infomationStr =  QObject::tr("系统初始化完成，请您操作(CANBUS) ");
        break;

    }

    }
    tmpMsg.MessageInformation = infomationStr.toStdString();
    MessageLog::getInstance()->addMessage(tmpMsg);

}

